#include <SPI.h>
#include <Wire.h>
#include <Adafruit_AMG88xx.h>
#include <TFT.h>

Adafruit_AMG88xx amg;

//定义引脚
#define cs 10
#define dc 9
#define rst 8

//定义温度范围
#define MINTEMP 40 
#define MAXTEMP 20

//定义插值边长
#define Length 11

//倒入camcolor[256]色表
const uint16_t camColors[] = {0x480F,
0x400F,0x400F,0x400F,0x4010,0x3810,0x3810,0x3810,0x3810,0x3010,0x3010,
0x3010,0x2810,0x2810,0x2810,0x2810,0x2010,0x2010,0x2010,0x1810,0x1810,
0x1811,0x1811,0x1011,0x1011,0x1011,0x0811,0x0811,0x0811,0x0011,0x0011,
0x0011,0x0011,0x0011,0x0031,0x0031,0x0051,0x0072,0x0072,0x0092,0x00B2,
0x00B2,0x00D2,0x00F2,0x00F2,0x0112,0x0132,0x0152,0x0152,0x0172,0x0192,
0x0192,0x01B2,0x01D2,0x01F3,0x01F3,0x0213,0x0233,0x0253,0x0253,0x0273,
0x0293,0x02B3,0x02D3,0x02D3,0x02F3,0x0313,0x0333,0x0333,0x0353,0x0373,
0x0394,0x03B4,0x03D4,0x03D4,0x03F4,0x0414,0x0434,0x0454,0x0474,0x0474,
0x0494,0x04B4,0x04D4,0x04F4,0x0514,0x0534,0x0534,0x0554,0x0554,0x0574,
0x0574,0x0573,0x0573,0x0573,0x0572,0x0572,0x0572,0x0571,0x0591,0x0591,
0x0590,0x0590,0x058F,0x058F,0x058F,0x058E,0x05AE,0x05AE,0x05AD,0x05AD,
0x05AD,0x05AC,0x05AC,0x05AB,0x05CB,0x05CB,0x05CA,0x05CA,0x05CA,0x05C9,
0x05C9,0x05C8,0x05E8,0x05E8,0x05E7,0x05E7,0x05E6,0x05E6,0x05E6,0x05E5,
0x05E5,0x0604,0x0604,0x0604,0x0603,0x0603,0x0602,0x0602,0x0601,0x0621,
0x0621,0x0620,0x0620,0x0620,0x0620,0x0E20,0x0E20,0x0E40,0x1640,0x1640,
0x1E40,0x1E40,0x2640,0x2640,0x2E40,0x2E60,0x3660,0x3660,0x3E60,0x3E60,
0x3E60,0x4660,0x4660,0x4E60,0x4E80,0x5680,0x5680,0x5E80,0x5E80,0x6680,
0x6680,0x6E80,0x6EA0,0x76A0,0x76A0,0x7EA0,0x7EA0,0x86A0,0x86A0,0x8EA0,
0x8EC0,0x96C0,0x96C0,0x9EC0,0x9EC0,0xA6C0,0xAEC0,0xAEC0,0xB6E0,0xB6E0,
0xBEE0,0xBEE0,0xC6E0,0xC6E0,0xCEE0,0xCEE0,0xD6E0,0xD700,0xDF00,0xDEE0,
0xDEC0,0xDEA0,0xDE80,0xDE80,0xE660,0xE640,0xE620,0xE600,0xE5E0,0xE5C0,
0xE5A0,0xE580,0xE560,0xE540,0xE520,0xE500,0xE4E0,0xE4C0,0xE4A0,0xE480,
0xE460,0xEC40,0xEC20,0xEC00,0xEBE0,0xEBC0,0xEBA0,0xEB80,0xEB60,0xEB40,
0xEB20,0xEB00,0xEAE0,0xEAC0,0xEAA0,0xEA80,0xEA60,0xEA40,0xF220,0xF200,
0xF1E0,0xF1C0,0xF1A0,0xF180,0xF160,0xF140,0xF100,0xF0E0,0xF0C0,0xF0A0,
0xF080,0xF060,0xF040,0xF020,0xF800,};

TFT tft = TFT(cs, dc, rst);

struct temp{
    uint8_t R;
    uint8_t G;
    uint8_t B;
};

//设置像素
uint16_t displayPixelWidth,
         displayPixelHeight;

float pixels2[Length*Length];

//uint16_t转RGB函数
struct temp u16_RGB(uint16_t x){
    struct temp ans;
    ans.R = x >> 11;
    x &=(~(ans.R<<11));
    ans.G = x >> 5;
    ans.B = x&(~(ans.G<<5));
    return ans;
}

void simulate(float* pixels){
    for (int i = 0; i < Length*Length;i++){
        int x = i / Length;
        int y = i % Length;
        int x1 = x * 7 / (Length-1);
        int y1 = y * 7 / (Length-1);
        float k = 1;
        float x2 = k * x * 7 / (Length - 1), y2 = k * y * 7 / (Length -1);
        if(x1<7&&y1<7)
          pixels2[i] = pixels[x1 * 8 + y1] * (x1 + 1 - x2) * (y1 + 1 - y2) + pixels[(x1 + 1) * 8 + y1] * (x2 - x1) * (y1 + 1 - y2) + pixels[x1 * 8 + y1 + 1] * (x1 + 1 - x2) * (y2 - y1) + pixels[x1 * 8 + y1] * (x2 - x1) * (y2 - y1);
        else if(x1 >=7&&y1<7)
          pixels2[i] = pixels[x1*8+y1]*(y1+1-y2)+pixels[x1*8+y1+1]*(y2-y1);
        else if(x1 <7&&y1>=7)
          pixels2[i] = pixels[x1*8+y1]*(x1+1-x2)+pixels[(x1+1)*8+y1]*(x2-x1);
        else pixels2[i]= pixels[x1*8+y1];
    }
}

void AMG_init(){

    //调整波特率
    Serial.begin(9600);
    Serial.println(F("AMG88xx pixels"));

    bool status;

    //初始化
    status = amg.begin();
    if(! status) {
        Serial.println("Could not find a valid sensor.");
        while(1);
    }

    //设计标题
    Serial.println("-- Pixels Test --");
    Serial.println();

    //等待传感器启动
    delay(100);
}

//读取矩阵函数
void Amg_read(){
    float pixels[64];
    amg.readPixels(pixels);
    Serial.println("------------Test-----------");
    simulate(pixels);
    /*    Serial.print("[");
    for (int i = 0; i < Length*Length;i++){
        Serial.print(pixels2[i]);
        Serial.print(", ");
        if( (i+1)%15 == 0)
            Serial.println();
    }
    Serial.println("]");
    Serial.println("---------------------------");*/
    //设置帧率()
    delay(100);
}

void TFT_init(){
    tft.begin();
    tft.background(0, 0, 0);
    displayPixelHeight = tft.height() / Length;
    displayPixelWidth = tft.width() /Length;
}

//显示函数
void screen(){
    for (int i = 0; i < Length*Length;i++){
        float kk = 1;
        int index = map(pixels2[i], MINTEMP, MAXTEMP, 0, 255);
        struct temp colorindex = u16_RGB(camColors[index]);
        tft.fill(colorindex.R, colorindex.G, colorindex.B);
        tft.rect(kk*displayPixelHeight * floor (i/Length),kk*displayPixelWidth * (i%Length),kk*displayPixelHeight, kk*displayPixelWidth);
    }
}

void setup(){
  AMG_init();
  TFT_init();
}

void loop(){
  Amg_read();
  screen();
}